package note;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.util.PIDController;

public class PIDLineFollow {
	public PIDController pidController;

	public float setpoint = 45;
	public float deadband = 1;
	public float kp = 10.f;
	public float ki = 1.0f;
	public float kd = 100.0f;
	public float limithigh = 200.0f;
	public float limitlow = -200.0f;
	public float integrallimithigh = 0.0f;
	public float integrallimitlow = 0.0f;
	public long dt = 25;

	public boolean isThreadStopped = true;
	public boolean stopped = false;

	public LightSensor sensor;
	public DifferentialPilot pilot;

	public PIDLineFollow() {
		sensor = new LightSensor(SensorPort.S1);
		sensor.setFloodlight(true);
		//2.1f, 4.4f, Motor.A,Motor.B, false
		pilot = new DifferentialPilot(2.1f, 4.4f, Motor.A, Motor.B, false);
		pilot.setTravelSpeed(30);
		pilot.setRotateSpeed(400);
	}

	public void setPIDParameters() {
		if (pidController != null) {
			pidController.setPIDParam(PIDController.PID_SETPOINT, setpoint);
			pidController.setPIDParam(PIDController.PID_DEADBAND, deadband);
			pidController.setPIDParam(PIDController.PID_KP, kp);
			pidController.setPIDParam(PIDController.PID_KI, ki);
			pidController.setPIDParam(PIDController.PID_KD, kd);
			pidController.setPIDParam(PIDController.PID_LIMITHIGH, limithigh);
			pidController.setPIDParam(PIDController.PID_LIMITLOW, limitlow);
			pidController.setPIDParam(PIDController.PID_I_LIMITHIGH,
					integrallimithigh);
			pidController.setPIDParam(PIDController.PID_I_LIMITLOW,
					integrallimitlow);
		}
	}

	public void start() {
		new Thread() {
			public void run() {
				isThreadStopped = false;

				pidController = new PIDController((int) setpoint, 100);
				setPIDParameters();

				while (!stopped) {
					int lightvalue = sensor.getLightValue();
					int MV = pidController.doPID(lightvalue);

					LCD.drawString(lightvalue + " " + MV + "  ", 0, 1);
					pilot.steer(MV);

					try {
						Thread.sleep(dt);
					} catch (InterruptedException e) {

					}
				}

				pilot.stop();

				isThreadStopped = true;
			}
		}.start();
	}

	public static void main(String argv[]) {

		PIDLineFollow p = new PIDLineFollow();
		p.stopped = false;
		p.start();
		try {
			while (!Button.ESCAPE.isDown()) {
				Thread.sleep(100);
			}
		} catch (Exception e) {

		}

		p.stopped = true;

	}

	public interface PIDConstants {

		public static int SHUTDOWN_COMMAND = -1;
		public static int STOP_COMMAND = 0;
		public static int START_COMMAND = 1;
		public static int READ_COMMAND = 2;
		public static int WRITE_COMMAND = 3;

		public static int LIGHTVALUE = 100;

		public static int TRAVEL_SPEED = 110;
		public static int ROTATE_SPEED = 111;
		public static int MAX_TRAVEL_SPEED = 112;
		public static int MAX_ROTATE_SPEED = 113;

		public static int INTERVAL = 120;
		public static int RAW_OUTPUT_MV = 121;
	}
}